#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <grid_map_msgs/GridMap.h>
#include <grid_map_ros/grid_map_ros.hpp>
#include <grid_map_cv/grid_map_cv.hpp>
#include <cv_bridge/cv_bridge.h>

#include <opencv2/imgcodecs.hpp>

#include "stair_ctrl/cv_to_csv.hpp"

#include <iostream>
#include <vector>
#include <string>
#include <filesystem>

namespace fs = std::filesystem;

int main(int argc, char **argv)
{
    if (argc == 1)
    {
        return 0;
    }

    std::string file = argv[1];
    auto pos = file.find_last_of('.');
    file = file.substr(0, pos);
    fs::path folder(file);
    if (!fs::exists(folder))
    {
        fs::create_directory(folder);
    }

    rosbag::Bag bag;
    bag.open(argv[1], rosbag::bagmode::Read);

    for(rosbag::MessageInstance const msg: rosbag::View(bag))
    {
        grid_map_msgs::GridMap::ConstPtr map_msg = msg.instantiate<grid_map_msgs::GridMap>();
        if (map_msg != nullptr)
        {
            std::stringstream ss;
            ss << "map_" << map_msg->info.header.stamp.sec << "_" << map_msg->info.header.stamp.nsec;
            
            grid_map::GridMap map;
            grid_map::GridMapRosConverter::fromMessage(*map_msg, map);

            auto layers = map.getLayers();
            for (const auto &layer: layers)
            {
                std::stringstream img_name;
                img_name << ss.str() << "_" << layer << ".png";
                
                cv_bridge::CvImage image;
                grid_map::GridMapRosConverter::toCvImage(map, layer, sensor_msgs::image_encodings::MONO16, -3.2767, 3.2768, image);
                ROS_INFO_STREAM("writting to " << folder / img_name.str());
                bool success = cv::imwrite((folder / img_name.str()).string(), image.image);
                ROS_INFO_STREAM("status: " << success);
                
                std::stringstream csv_name;
                csv_name << ss.str() << "_" << layer << ".csv";
                cv_to_csv((folder / csv_name.str()).string(), image.image);
            }
        }
    }

    bag.close();
};
